﻿/* ------------------------------------------------------------------------- */
/*
 * C O M P A N Y   P R O P R I E T A R Y
 * use or disclosure of data contained in this file is subject to the
 * requirements an the end of this file
 *
 * CLASSIFICATION: OFFEN / UNCLASSIFIED
 */
/* ------------------------------------------------------------------------- */

/* ######################################################################### */
/** \file M44_HeightGradientEst.cs
 * \brief	This file contains the Height_Gradient_Estimation function that estimates 
 *          the heights and gradients of each cell of the map after all the neccesary 
 *          information has been recolected.
 *  
 * This file contains the Height_Gradient_Estimation function that estimates the 
 * heights and gradients of each cell of the map after all the neccesary information 
 * has been recolected.
 * 
 * PROJEKT:   NETWORKED MULTI-ROBOT SYSTEMS
 *
 * Copyright (c) 2008 SENER - Universidad Politécnica de Madrid.
 *
 * \author    Antonio Matta \n
 *            Universidad Politécnica de Madrid \n
 *            Grupo de Robótica y Cibernética \n
 * \version   0.01
 * \date      2009-09-01
 *
 * \n \n
 * Versionshistory: \n
 * -----------------
 * - Version 0.01:   Antonio MATTA                          2009-09-01 \n
 *      First implementation of the Height_Gradient_Estimation function.
 *      
 * - Version 0.02:   Antonio MATTA                          2010-01-13 \n
 *      Gradient Estimation set to -2.0.
 *      
 * - Version 0.03:   Antonio MATTA                          2010-02-26 \n
 *      Mean gradient calculation added.
 *      
/* ######################################################################### */

using System;
using System.Collections.Generic;
using System.Text;

/*Using the C Sharp Matrix Library*/
using NMRS.CSML;

/*Using Interfaces classes*/
using Interfaces;
using Interfaces.Complex;
using Interfaces.Basis;

/*This is for using lists*/
using System.Collections;

/* ------------------------------------------------------------------------- */
/**	\namespace NMRS.M44_SensorFusionAndMapping
 *
 *  \brief	This is the namespace of the M44 Sensor Fusion and Mapping module
 */
/* ------------------------------------------------------------------------- */
namespace NMRS.M44_SensorFusionAndMapping
{
    /* ------------------------------------------------------------------------- */
    /**	\class M44_HeightGradientEst
     *
     *  \brief  This is the class of the M44_HeightGradientEst function.
     * 
     */
    /* ------------------------------------------------------------------------- */
    static class M44_HeightGradientEst
    {
        /* ------------------------------------------------------------------------- */
        /**	public static void Height_Gradient_Estimation( ArrayList[,] Cell_List,
         *                                                 aux_MapCell aux_MapCell_local
         *                                                )
         * 
         *  \brief 
         *  
         *  \param[in] ArrayList[,] Cell_List
         *  \param[in] aux_MapCell aux_MapCell_local
         *
         */
        /* ------------------------------------------------------------------------- */
        public static aux_MapCell Height_Gradient_Estimation(ArrayList[,] Cell_List,
                                                              aux_MapCell aux_MapCell_local
                                                            )
        {
            float y = 0.0f;
            float mean = 0.0f;

            /*We check which cells have been explored by the sensors*/
            for (int i = 0; i < aux_MapCell_local.us_Nrows; i++)
            {
                for (int j = 0; j < aux_MapCell_local.us_Ncols; j++)
                {
                    if (Cell_List[i, j] != null)
                    {
                        /*Computation of the mean for each cell*/
                        y = 0.0f;
                        foreach (cl_Pos3D pos in Cell_List[i, j])
                        {
                            y =+ pos.f_y;
                        }

                        mean = y / Cell_List[i, j].Count;

                        aux_MapCell_local.Measured_Heights[i, j] = mean;

                        if (Cell_List[i, j].Count > 2)
                        {
                            int n = Cell_List[i, j].Count;
                            Matrix ones = Matrix.Ones(n, 1);
                            Matrix f_x = new Matrix(n, 1);
                            Matrix f_y = new Matrix(n, 1);
                            Matrix f_z = new Matrix(n, 1);

                            for (int k = 0; k < n; k++)
                            {
                                cl_Pos3D pos = new cl_Pos3D();
                                pos = (cl_Pos3D)Cell_List[i, j][k];
                                f_x[k+1, 1].Re = pos.f_x;
                                f_y[k+1, 1].Re = pos.f_y;
                                f_z[k+1, 1].Re = pos.f_z;
                                
                            }

                            Matrix M_temp = Matrix.VerticalConcat(ones, f_x);
                            Matrix M = Matrix.VerticalConcat(M_temp, f_y);

                            
                            Matrix tmp_M = M.Transpose() * M;
                            
                            Complex Determinant;
                            Determinant = tmp_M.Determinant();

                            if (Determinant == Complex.Zero)
                            {
                                break;
                            }
                            
                            Matrix P_M = tmp_M.Inverse() * M.Transpose();
                            Matrix Beta = P_M * f_z;

                            aux_MapCell_local.Measured_Gradient_X[i, j] = (float)Beta[2, 1].Re;
                            aux_MapCell_local.Measured_Gradient_Y[i, j] = (float)Beta[3, 1].Re;

                        }
                    }
                }
            }
            return aux_MapCell_local;
        }
    }
}

/* ------------------------------------------------------------------------- */
/*
 * RIGHT OF USE. This document may neither be passed on to third parties or
 * reproduced nor its contents utilized or divulged without the expressed
 * prior permission of the EUROPEAN DEFENCE AGENCY, or any national government
 * having rights on it. In case of contravention, the offender shall be
 * liable for damages.
 *
 * ATTENTION! DEFENCE MATERIAL. This document may contain data which is subject
 * to export control. For the export of this data an export license is
 * required.
 *
 * COMPANY PROPRIETARY. This document contains proprietary information and
 * may only be used by the recipient for the prescribed purposes and may
 * neither be reproduced in any form nor the document itself or its content
 * divulged to third parties without our expressed prior written permission.
 *
 * COPYRIGHT (C) Diehl BGT Defence GmbH & Co. KG; 2008; All rights reserved; 
 *
 * DISTRIBUTION AND USAGE RIGHTS: Restricted to the NETWORKED MULTI-ROBOT
 * SYSTEMS Project Consortium, participation governments, prime contractor,
 * subcontractors and vendors in accordance with contract / subcontract
 * clauses and / or other specified written agreements.
 */
/* ------------------------------------------------------------------------- */
